//由于调试的时候发现HC-SR04会干扰到dmp库读取欧拉角，所以暂时将HC-SR04取消
#include "stm32f1xx_hal.h"
//#include "Delay.h"
#include "PID.h"
#include "MPU6050.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
//#define   HCSR04_TrigPort   GPIOA
//#define   HCSR04_EchoPort   GPIOA
//#define   HCSR04_TrigPin    GPIO_PIN_3
//#define   HCSR04_EchoPin    GPIO_PIN_2

//int Counter = 0;
//float Distance;
//extern TIM_HandleTypeDef htim3;
extern float Pitch, Roll, Yaw; //外部引用欧拉角
//void HC_SR04_Start(void)
//{
//  HAL_GPIO_WritePin(HCSR04_TrigPort, HCSR04_TrigPin, GPIO_PIN_SET);
//  Delay_us(13);
//  HAL_GPIO_WritePin(HCSR04_TrigPort, HCSR04_TrigPin, GPIO_PIN_RESET);
//}

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)  //MPU6050的INT中断服务函数，由于MPU设置为10ms采样一次，所以该函数没10ms执行一次
{
  if(mpu_dmp_get_data(&Pitch, &Roll, &Yaw) == 0)//当dmp可以正确读取欧拉角的时候
  {
    if(GPIO_Pin == GPIO_PIN_5)
    {
      PID_Control();                            //立即进行一次PID运算
    }
  }
}



//  if(GPIO_Pin == GPIO_PIN_2)
//  {
//    if(HAL_GPIO_ReadPin(HCSR04_EchoPort, HCSR04_EchoPin) == GPIO_PIN_SET)
//    {
//      __HAL_TIM_SET_COUNTER(&htim3, 0);
//      HAL_TIM_Base_Start(&htim3);
//    }
//    if(HAL_GPIO_ReadPin(HCSR04_EchoPort, HCSR04_EchoPin) == GPIO_PIN_RESET)
//    {
//      HAL_TIM_Base_Stop(&htim3);
//      Counter = __HAL_TIM_GET_COUNTER(&htim3);
//      Distance = Counter * 0.017; //单位是厘米
//    }  
//  }
